#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "nvs_flash.h"
#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_gatts_api.h"
#include "esp_gap_ble_api.h"
#include "esp_gatt_common_api.h"
#include "esp_log.h"

#include "app_ble.h"
#include "app_data.h"
#include "bsp_ota.h"

#define TAG "app_ble"

#define PROFILE_NUM 1
#define PROFILE_APP_IDX 0
#define ESP_APP_ID 0x55
#define SAMPLE_DEVICE_NAME NMAE
#define SVC_INST_ID 0

#define GATTS_DEMO_CHAR_VAL_LEN_MAX 500
#define PREPARE_BUF_MAX_SIZE 1024
#define CHAR_DECLARATION_SIZE (sizeof(uint8_t))
#define ADV_CONFIG_FLAG (1 << 0)
#define SCAN_RSP_CONFIG_FLAG (1 << 1)

struct gatts_profile_inst
{
	esp_gatts_cb_t gatts_cb;
	uint16_t gatts_if;
	uint16_t app_id;
	uint16_t conn_id;
	uint16_t service_handle;
	esp_gatt_srvc_id_t service_id;
	uint16_t char_handle;
	esp_bt_uuid_t char_uuid;
	esp_gatt_perm_t perm;
	esp_gatt_char_prop_t property;
	uint16_t descr_handle;
	esp_bt_uuid_t descr_uuid;
};

static const char *NMAE;
static const char BLE_NAME[20] = "JC-hud";
static uint16_t hud_handle_table[HRS_IDX_NB];

static uint8_t adv_config_done = 0;
static bool update_firmware = false;
static update_data_t update_data;
static navigate_data_t navigate_data;
static prepare_type_env_t prepare_write_env;
QueueHandle_t update_msg_queue = NULL;
QueueHandle_t navigate_msg_queue = NULL;
SemaphoreHandle_t update_stop_sem = NULL;

static uint8_t hud_service_uuid[16] = {0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e};
static uint8_t hud_char_notify_uuid[16] = {0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e};
static uint8_t hud_char_write_uuid[16] = {0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e};
/* Service uuid */
static const uint16_t primary_service_uuid = ESP_GATT_UUID_PRI_SERVICE;
static const uint16_t character_declaration_uuid = ESP_GATT_UUID_CHAR_DECLARE;
static const uint16_t character_client_config_uuid = ESP_GATT_UUID_CHAR_CLIENT_CONFIG;
/* type define */
static const uint8_t char_prop_notify = ESP_GATT_CHAR_PROP_BIT_NOTIFY;
static uint8_t char_prop_write_write_nr = ESP_GATT_CHAR_PROP_BIT_WRITE | ESP_GATT_CHAR_PROP_BIT_WRITE_NR;
// static uint8_t char_prop_read_notify = ESP_GATT_CHAR_PROP_BIT_READ|ESP_GATT_CHAR_PROP_BIT_NOTIFY;
// static uint8_t char_prop_read_write = ESP_GATT_CHAR_PROP_BIT_WRITE_NR|ESP_GATT_CHAR_PROP_BIT_READ;
// static const uint8_t char_prop_read_write_notify = ESP_GATT_CHAR_PROP_BIT_WRITE | ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_NOTIFY;

/* Define characteristic variables */
static const uint8_t hud_notify_val[100] = {0x00};
static const uint8_t hud_notify_ccc[] = {0x00, 0x00};
static const uint8_t hud_write_val[100] = {0x00};

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param);
/* One gatt-based profile one app_id and one gatts_if, this array will store the gatts_if returned by ESP_GATTS_REG_EVT */
static struct gatts_profile_inst hud_profile_tab[PROFILE_NUM] = {
	[PROFILE_APP_IDX] = {
		.gatts_cb = gatts_profile_event_handler,
		.gatts_if = ESP_GATT_IF_NONE, /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */
	},
};
/* The length of adv data must be less than 31 bytes */
static esp_ble_adv_data_t adv_data = {
	.set_scan_rsp = false,
	.include_name = true,
	.include_txpower = true,
	.min_interval = 0x0006, //slave connection min interval, Time = min_interval * 1.25 msec
	.max_interval = 0x0010, //slave connection max interval, Time = max_interval * 1.25 msec
	.appearance = 0x00,
	.manufacturer_len = 0,		 //TEST_MANUFACTURER_DATA_LEN,
	.p_manufacturer_data = NULL, //test_manufacturer,
	.service_data_len = 0,
	.p_service_data = NULL,
	.service_uuid_len = sizeof(hud_service_uuid),
	.p_service_uuid = hud_service_uuid,
	.flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT),
};
// scan response data
static esp_ble_adv_data_t scan_rsp_data = {
	.set_scan_rsp = true,
	.include_name = true,
	.include_txpower = true,
	.min_interval = 0x0006,
	.max_interval = 0x0010,
	.appearance = 0x00,
	.manufacturer_len = 0,		 //TEST_MANUFACTURER_DATA_LEN,
	.p_manufacturer_data = NULL, //&test_manufacturer[0],
	.service_data_len = 0,
	.p_service_data = NULL,
	.service_uuid_len = sizeof(hud_service_uuid),
	.p_service_uuid = hud_service_uuid,
	.flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT),
};

static esp_ble_adv_params_t adv_params = {
	.adv_int_min = 0x20,
	.adv_int_max = 0x40,
	.adv_type = ADV_TYPE_IND,
	.own_addr_type = BLE_ADDR_TYPE_PUBLIC,
	.channel_map = ADV_CHNL_ALL,
	.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};

/* Full Database Description - Used to add attributes into the database */
static const esp_gatts_attr_db_t gatt_db[HRS_IDX_NB] = {
	//Service Declaration
	[IDX_SVC] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ, sizeof(hud_service_uuid), sizeof(hud_service_uuid), (uint8_t *)hud_service_uuid}},
	//data notify characteristic Declaration
	[IDX_CHAR_A] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_notify}},
	//data notify characteristic
	[IDX_CHAR_VAL_A] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_128, (uint8_t *)hud_char_notify_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, GATTS_DEMO_CHAR_VAL_LEN_MAX, sizeof(hud_notify_val), (uint8_t *)hud_notify_val}},
	//data notify characteristic - Client Characteristic Configuration Descriptor
	[IDX_CHAR_CFG_A] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(hud_notify_ccc), (uint8_t *)hud_notify_ccc}},
	//data write characteristic Declaration
	[IDX_CHAR_B] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_write_write_nr}},
	//data write characteristic Value
	[IDX_CHAR_VAL_B] =
		{{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_128, (uint8_t *)hud_char_write_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, GATTS_DEMO_CHAR_VAL_LEN_MAX, sizeof(hud_write_val), (uint8_t *)hud_write_val}},
};

static void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
{
	switch (event)
	{
	case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT:
		adv_config_done &= (~ADV_CONFIG_FLAG);
		if (adv_config_done == 0)
		{
			esp_ble_gap_start_advertising(&adv_params);
		}
		break;
	case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT:
		adv_config_done &= (~SCAN_RSP_CONFIG_FLAG);
		if (adv_config_done == 0)
		{
			esp_ble_gap_start_advertising(&adv_params);
		}
		break;
	case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
		/* advertising start complete event to indicate advertising start successfully or failed */
		if (param->adv_start_cmpl.status != ESP_BT_STATUS_SUCCESS)
		{
			ESP_LOGE(TAG, "advertising start failed");
		}
		else
		{
			ESP_LOGD(TAG, "advertising start successfully");
		}
		break;
	case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:
		if (param->adv_stop_cmpl.status != ESP_BT_STATUS_SUCCESS)
		{
			ESP_LOGE(TAG, "Advertising stop failed");
		}
		else
		{
			ESP_LOGD(TAG, "Stop adv successfully");
		}
		break;
	case ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT:
		ESP_LOGD(TAG, "update connection params status = %d, min_int = %d, max_int = %d,conn_int = %d,latency = %d, timeout = %d",
				 param->update_conn_params.status,
				 param->update_conn_params.min_int,
				 param->update_conn_params.max_int,
				 param->update_conn_params.conn_int,
				 param->update_conn_params.latency,
				 param->update_conn_params.timeout);
		break;
	default:
		break;
	}
}

static void example_prepare_write_event_env(esp_gatt_if_t gatts_if, prepare_type_env_t *prepare_write_env, esp_ble_gatts_cb_param_t *param)
{
	ESP_LOGD(TAG, "prepare write, handle = %d, value len = %d", param->write.handle, param->write.len);
	esp_gatt_status_t status = ESP_GATT_OK;
	if (prepare_write_env->prepare_buf == NULL)
	{
		prepare_write_env->prepare_buf = (uint8_t *)malloc(PREPARE_BUF_MAX_SIZE * sizeof(uint8_t));
		printf("prepare_wirte_env->prepare_buf addr_check: %p\n", prepare_write_env->prepare_buf);

		prepare_write_env->prepare_len = 0;
		if (prepare_write_env->prepare_buf == NULL)
		{
			ESP_LOGE(TAG, "%s, Gatt_server prep no mem", __func__);
			status = ESP_GATT_NO_RESOURCES;
		}
	}
	else
	{
		if (param->write.offset > PREPARE_BUF_MAX_SIZE)
		{
			status = ESP_GATT_INVALID_OFFSET;
		}
		else if ((param->write.offset + param->write.len) > PREPARE_BUF_MAX_SIZE)
		{
			status = ESP_GATT_INVALID_ATTR_LEN;
		}
	}
	/*send response when param->write.need_rsp is true */
	if (param->write.need_rsp)
	{
		esp_gatt_rsp_t *gatt_rsp = (esp_gatt_rsp_t *)malloc(sizeof(esp_gatt_rsp_t));
		printf("gatt_rsp addr_check: %p\n", gatt_rsp);
		if (gatt_rsp != NULL)
		{
			gatt_rsp->attr_value.len = param->write.len;
			gatt_rsp->attr_value.handle = param->write.handle;
			gatt_rsp->attr_value.offset = param->write.offset;
			gatt_rsp->attr_value.auth_req = ESP_GATT_AUTH_REQ_NONE;
			memcpy(gatt_rsp->attr_value.value, param->write.value, param->write.len);
			esp_err_t response_err = esp_ble_gatts_send_response(gatts_if, param->write.conn_id, param->write.trans_id, status, gatt_rsp);
			if (response_err != ESP_OK)
			{
				ESP_LOGE(TAG, "Send response error");
			}
			free(gatt_rsp);
		}
		else
		{
			ESP_LOGE(TAG, "%s, malloc_psram failed", __func__);
		}
	}
	if (status != ESP_GATT_OK)
	{
		return;
	}
	memcpy(prepare_write_env->prepare_buf + param->write.offset,
		   param->write.value,
		   param->write.len);
	prepare_write_env->prepare_len += param->write.len;
}

static void example_exec_write_event_env(prepare_type_env_t *prepare_write_env, esp_ble_gatts_cb_param_t *param)
{
	if (param->exec_write.exec_write_flag == ESP_GATT_PREP_WRITE_EXEC && prepare_write_env->prepare_buf)
	{
		esp_log_buffer_hex(TAG, prepare_write_env->prepare_buf, prepare_write_env->prepare_len);
	}
	else
	{
		ESP_LOGD(TAG, "ESP_GATT_PREP_WRITE_CANCEL");
	}
	if (prepare_write_env->prepare_buf)
	{
		free(prepare_write_env->prepare_buf);
		prepare_write_env->prepare_buf = NULL;
	}
	prepare_write_env->prepare_len = 0;
}

uint8_t app_ble_gatt_send(uint8_t *val, uint16_t len)
{
	esp_gatt_if_t gatts_if = 3;
	uint16_t conn_id = 0;
	return esp_ble_gatts_send_indicate(gatts_if, conn_id,
									   hud_handle_table[IDX_CHAR_VAL_A],
									   len,
									   val,
									   true);
}

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
	switch (event)
	{
	case ESP_GATTS_REG_EVT:
	{
		NMAE = BLE_NAME;
		esp_err_t set_dev_name_ret = esp_ble_gap_set_device_name(SAMPLE_DEVICE_NAME);
		if (set_dev_name_ret)
		{
			ESP_LOGE(TAG, "set device name failed, error code = %x", set_dev_name_ret);
		}
		//config adv data
		esp_err_t ret = esp_ble_gap_config_adv_data(&adv_data);
		if (ret)
		{
			ESP_LOGE(TAG, "config adv data failed, error code = %x", ret);
		}
		adv_config_done |= ADV_CONFIG_FLAG;
		//config scan response data
		ret = esp_ble_gap_config_adv_data(&scan_rsp_data);
		if (ret)
		{
			ESP_LOGE(TAG, "config scan response data failed, error code = %x", ret);
		}
		adv_config_done |= SCAN_RSP_CONFIG_FLAG;
		esp_err_t create_attr_ret = esp_ble_gatts_create_attr_tab(gatt_db, gatts_if, HRS_IDX_NB, SVC_INST_ID);
		ESP_LOGE(TAG, "timo create attr tab!:%d\r", HRS_IDX_NB);
		if (create_attr_ret)
		{
			ESP_LOGE(TAG, "create attr table failed, error code = %x", create_attr_ret);
		}
	}
	break;
	case ESP_GATTS_READ_EVT:
		ESP_LOGD(TAG, "ESP_GATTS_READ_EVT");
		printf("handle = %d,gatt_if = %d,connid=%d,handle_table[%d]\r\n", param->read.handle, gatts_if, param->read.conn_id, hud_handle_table[IDX_CHAR_VAL_A]);
		break;
	case ESP_GATTS_WRITE_EVT:
		if (!param->write.is_prep)
		{
			if (hud_handle_table[IDX_CHAR_VAL_B] == param->write.handle)
			{
				if (param->write.len == 2)
				{
					uint16_t descr_value = param->write.value[1] << 8 | param->write.value[0];

					if (descr_value == 0x3412 && update_firmware == false)
					{
						//开始更新固件
						ESP_LOGI(TAG, "Start to update the firmware");

						//创建接收线程
						if (xSemaphoreTake(update_stop_sem, (TickType_t)1000 / portTICK_PERIOD_MS) != pdTRUE)
						{
							ESP_LOGI(TAG, "update_stop_sem handle is locked");
							break;
						}
						update_firmware = true;
						update_data.len = 0;
						xTaskCreate(bsp_ota_begin_task, "bsp_ota_begin_task", 20480, NULL, 1, NULL);
					}
					else if (descr_value == 0x7856 && update_firmware == true)
					{
						//停止更新固件
						ESP_LOGI(TAG, "Stop updating firmware");
						update_firmware = false;
						//给更新程序发送完成信号量
						//发送剩余数据
						xQueueSend(update_msg_queue, &update_data, portMAX_DELAY); //传拷贝不传指针
						xSemaphoreGive(update_stop_sem);
					}
					else
					{
						ESP_LOGI(TAG, "Invalid command");
					}
				}
				else
				{
					if (update_firmware == true)
					{
						//升级数据、数据拼接、判断是否退出烧录
						if (512 - update_data.len < param->write.len)
						{
							xQueueSend(update_msg_queue, &update_data, portMAX_DELAY); //传拷贝不传指针
							update_data.len = 0;
						}
						uint8_t *pointer = update_data.value + update_data.len;
						memcpy(pointer, param->write.value, param->write.len);
						update_data.len += param->write.len;
					}
					else
					{
						//导航数据、将数据提交给导航数据解析函数
						// ESP_LOGI(TAG, "Send navigate message %d", param->write.len);
						// ESP_LOGI(TAG, "Submit the data to the navigation data analysis function");
						navigate_data.len = param->write.len;
						memcpy(navigate_data.value, param->write.value, param->write.len);
						xQueueSend(navigate_msg_queue, &navigate_data, portMAX_DELAY);
					}
				}
			}

			ESP_LOGD(TAG, "ESP_GATTS_READ_EVT,handle = %d,gatt_if = %d,connid=%d,handle_table[%d]", param->write.handle, gatts_if, param->write.conn_id, hud_handle_table[IDX_CHAR_VAL_B]);
			if (hud_handle_table[IDX_CHAR_CFG_A] == param->write.handle && param->write.len == 2)
			{
				uint16_t descr_value = param->write.value[1] << 8 | param->write.value[0];
				if (descr_value == 0x0001)
				{
					ESP_LOGD(TAG, "notify enable");
				}
				else if (descr_value == 0x0002)
				{
					ESP_LOGD(TAG, "indicate enable");
				}
				else if (descr_value == 0x0000)
				{
					ESP_LOGD(TAG, "notify/indicate disable ");
				}
				else
				{
					ESP_LOGE(TAG, "unknown descr value");
					esp_log_buffer_hex(TAG, param->write.value, param->write.len);
				}
			}

			/* send response when param->write.need_rsp is true*/
			if (param->write.need_rsp)
			{
				esp_ble_gatts_send_response(gatts_if, param->write.conn_id, param->write.trans_id, ESP_GATT_OK, NULL);
			}
		}
		else
		{
			/* handle prepare write */
			example_prepare_write_event_env(gatts_if, &prepare_write_env, param);
		}
		break;
	case ESP_GATTS_EXEC_WRITE_EVT:
		// the length of gattc prepare write data must be less than GATTS_DEMO_CHAR_VAL_LEN_MAX.
		ESP_LOGD(TAG, "ESP_GATTS_EXEC_WRITE_EVT");
		example_exec_write_event_env(&prepare_write_env, param);
		break;
	case ESP_GATTS_MTU_EVT:
		ESP_LOGD(TAG, "ESP_GATTS_MTU_EVT, MTU %d", param->mtu.mtu);
		break;
	case ESP_GATTS_CONF_EVT:
		/* 当收到确认时，事件发生 */
		ESP_LOGD(TAG, "ESP_GATTS_CONF_EVT, status = %d, attr_handle %d", param->conf.status, param->conf.handle);
		break;
	case ESP_GATTS_START_EVT:
		ESP_LOGD(TAG, "SERVICE_START_EVT, status %d, service_handle %d", param->start.status, param->start.service_handle);
		break;
	case ESP_GATTS_CONNECT_EVT:
		app_data.ble_enable = 1;
		ESP_LOGD(TAG, "ESP_GATTS_CONNECT_EVT, conn_id = %d", param->connect.conn_id);
		esp_log_buffer_hex(TAG, param->connect.remote_bda, 6);
		esp_ble_conn_update_params_t conn_params = {0};
		memcpy(conn_params.bda, param->connect.remote_bda, sizeof(esp_bd_addr_t));
		/* For the iOS system, please refer to Apple official documents about the BLE connection parameters restrictions. */
		conn_params.latency = 0;
		conn_params.max_int = 0x20;	 // max_int = 0x20*1.25ms = 40ms
		conn_params.min_int = 0x10;	 // min_int = 0x10*1.25ms = 20ms
		conn_params.timeout = 0x190; // timeout = 400*10ms = 4000ms
		//start sent the update connection parameters to the peer device.
		esp_ble_gap_update_conn_params(&conn_params);
		ESP_LOGW(TAG, "BLE Connected!");
		break;
	case ESP_GATTS_DISCONNECT_EVT:
		app_data.ble_enable = 0;
		memset(&app_data.nav, 0, sizeof(nav_info_t));
		ESP_LOGD(TAG, "ESP_GATTS_DISCONNECT_EVT, reason = 0x%x", param->disconnect.reason);
		esp_ble_gap_start_advertising(&adv_params);
		/*蓝牙断开连接*/
		ESP_LOGW(TAG, "BLE Disonnected!");
		break;
	case ESP_GATTS_CREAT_ATTR_TAB_EVT:
	{
		if (param->add_attr_tab.status != ESP_GATT_OK)
		{
			ESP_LOGE(TAG, "create attribute table failed, error code=0x%x", param->add_attr_tab.status);
		}
		else if (param->add_attr_tab.num_handle != HRS_IDX_NB)
		{
			ESP_LOGE(TAG, "create attribute table abnormally, num_handle (%d) \
                        doesn't equal to HRS_IDX_NB(%d)",
					 param->add_attr_tab.num_handle, HRS_IDX_NB);
		}
		else
		{
			ESP_LOGI(TAG, "create attribute table successfully, the number handle = %d", param->add_attr_tab.num_handle);
			memcpy(hud_handle_table, param->add_attr_tab.handles, sizeof(hud_handle_table));
			esp_ble_gatts_start_service(hud_handle_table[IDX_SVC]);
		}
		break;
	}
	case ESP_GATTS_STOP_EVT:
	case ESP_GATTS_OPEN_EVT:
	case ESP_GATTS_CANCEL_OPEN_EVT:
	case ESP_GATTS_CLOSE_EVT:
	case ESP_GATTS_LISTEN_EVT:
	case ESP_GATTS_CONGEST_EVT:
	case ESP_GATTS_UNREG_EVT:
	case ESP_GATTS_DELETE_EVT:
	default:
		break;
	}
}

static void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{

	/* If event is register event, store the gatts_if for each profile */
	if (event == ESP_GATTS_REG_EVT)
	{
		if (param->reg.status == ESP_GATT_OK)
		{
			hud_profile_tab[PROFILE_APP_IDX].gatts_if = gatts_if;
		}
		else
		{
			ESP_LOGE(TAG, "reg app failed, app_id %04x, status %d",
					 param->reg.app_id,
					 param->reg.status);
			return;
		}
	}
	do
	{
		int idx;
		for (idx = 0; idx < PROFILE_NUM; idx++)
		{
			/* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
			if (gatts_if == ESP_GATT_IF_NONE || gatts_if == hud_profile_tab[idx].gatts_if)
			{
				if (hud_profile_tab[idx].gatts_cb)
				{
					hud_profile_tab[idx].gatts_cb(event, gatts_if, param);
				}
			}
		}
	} while (0);
}

void app_ble_gatts_init(void)
{
	esp_err_t ret;

	navigate_msg_queue = xQueueCreate(4, 512);
	if (navigate_msg_queue == 0)
	{
		printf("sys_msg_queue creat fail\r\n");
	}

	ret = esp_ble_gatts_register_callback(gatts_event_handler);
	if (ret)
	{
		ESP_LOGE(TAG, "gatts register error, error code = %x", ret);
		return;
	}

	ret = esp_ble_gap_register_callback(gap_event_handler);
	if (ret)
	{
		ESP_LOGE(TAG, "gap register error, error code = %x", ret);
		return;
	}

	ret = esp_ble_gatts_app_register(ESP_APP_ID);
	if (ret)
	{
		ESP_LOGE(TAG, "gatts app register error, error code = %x", ret);
		return;
	}

	esp_err_t local_mtu_ret = esp_ble_gatt_set_local_mtu(517);
	if (local_mtu_ret)
	{
		ESP_LOGE(TAG, "set local  MTU failed, error code = %x", local_mtu_ret);
	}
}

void app_ble_task(void *arg)
{
	esp_err_t ret;

	update_stop_sem = xSemaphoreCreateBinary();
	if (update_stop_sem == NULL)
	{
		printf("sys_msg_queue creat fail\r\n");
	}
	else
	{
		xSemaphoreGive(update_stop_sem);
	}

	navigate_msg_queue = xQueueCreate(5, sizeof(navigate_data_t));
	if (navigate_msg_queue == 0)
	{
		printf("sys_msg_queue creat fail\r\n");
	}

	printf("free_heap_size = %d\n", esp_get_free_heap_size());
	vTaskDelay(4000 / portTICK_PERIOD_MS);

	update_msg_queue = xQueueCreate(5, sizeof(update_data_t));
	if (update_msg_queue == 0)
	{
		printf("sys_msg_queue creat fail\r\n");
	}

	printf("free_heap_size = %d\n", esp_get_free_heap_size());
	vTaskDelay(4000 / portTICK_PERIOD_MS);

	ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));
	// esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);

	esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
	ret = esp_bt_controller_init(&bt_cfg);
	if (ret)
	{
		ESP_LOGE(TAG, "%s initialize controller failed: %s", __func__, esp_err_to_name(ret));
		return;
	}

	ret = esp_bt_controller_enable(ESP_BT_MODE_BLE);
	if (ret)
	{
		ESP_LOGE(TAG, "%s enable controller failed: %s", __func__, esp_err_to_name(ret));
		return;
	}

	ret = esp_bluedroid_init();
	if (ret)
	{
		ESP_LOGE(TAG, "%s init bluetooth failed: %s", __func__, esp_err_to_name(ret));
		return;
	}

	ret = esp_bluedroid_enable();
	if (ret)
	{
		ESP_LOGE(TAG, "%s enable bluetooth failed: %s", __func__, esp_err_to_name(ret));
		return;
	}

	ret = esp_ble_gatts_register_callback(gatts_event_handler);
	if (ret)
	{
		ESP_LOGE(TAG, "gatts register error, error code = %x", ret);
		return;
	}
	ret = esp_ble_gap_register_callback(gap_event_handler);
	if (ret)
	{
		ESP_LOGE(TAG, "gap register error, error code = %x", ret);
		return;
	}

	ret = esp_ble_gatts_app_register(ESP_APP_ID);
	if (ret)
	{
		ESP_LOGE(TAG, "gatts app register error, error code = %x", ret);
		return;
	}

	esp_err_t local_mtu_ret = esp_ble_gatt_set_local_mtu(500);
	if (local_mtu_ret)
	{
		ESP_LOGE(TAG, "set local  MTU failed, error code = %x", local_mtu_ret);
	}

	// ble_gatts_init();

	while (1)
	{
		vTaskDelay(2000 / portTICK_PERIOD_MS);
	}
}